#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/bool.hpp>
#include <mavros_msgs/msg/rc_in.hpp>
#include <customized_msgs/msg/gcs_cmd.hpp>
#include <filesystem>
#include <signal.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <unistd.h>
#include <sys/statvfs.h> 
#include <sys/stat.h>  
#include <vector>

#define ANSI_COLOR_RED     "\x1b[31m"
#define ANSI_COLOR_GREEN   "\x1b[32m"
#define ANSI_COLOR_BLUE    "\x1b[34m"
#define USE_WHITE_OR_BLACK_LIST 1 //0 for white 1 for black
namespace fs = std::filesystem;

class RosbagRecorder : public rclcpp::Node
{
public:
    RosbagRecorder()
        : Node("rosbag_recorder"), is_recording_(false), process_pid_(-1)
    {
      
        gcs_cmd_sub = this->create_subscription<customized_msgs::msg::GcsCmd>(
            "/winch_wireless_server/gcs_cmd", 10, std::bind(&RosbagRecorder::triggerCallback, this, std::placeholders::_1));
        RCLCPP_INFO(this->get_logger(), "start RosbagRecorder ");
        startRecording();
        RCLCPP_INFO(this->get_logger(), "start sh ");
    }

    void triggerCallback(const customized_msgs::msg::GcsCmd::SharedPtr msg)
    {   // {"takeoff", 3},
        
        if (msg->cmd == 14 && is_recording_)
        {
            RCLCPP_INFO(this->get_logger(), "triggerCallback  cmd %d is_recording_ %d",msg->cmd,is_recording_);
            stopRecording();

            // RCLCPP_INFO(this->get_logger(), "stopRecording ");
            // startRecording();
        }
    }


    void startRecording()
    {
        RCLCPP_INFO(this->get_logger(), "Starting rosbag recording...");
        process_pid_ = fork();

        int max_log_number = getMaxLogNumber();
        double free_space = get_free_space();
        RCLCPP_INFO(this->get_logger(), "Free space: %f GB", free_space);

        int delete_log_num = 0;
        while (free_space < 35)
        {
            delete_oldest_winch_log(free_space);
            RCLCPP_INFO(this->get_logger(), "Free space: %f", free_space);
            delete_log_num++;
            if (delete_log_num > 5)
            {
                RCLCPP_ERROR(this->get_logger(), "delete more than 5 winch logs, but still not enough space: %f", free_space);
                break;
            }
        }
        
        std::string bag_name =bag_dir+ "/winch_log_" + std::to_string(max_log_number + 1) + ".bag";
        RCLCPP_INFO(this->get_logger(), "my process_pid_ %d bag_name %s  ",process_pid_,bag_name.c_str());
        if (process_pid_ == 0)
        {
            setpgid(0, 0);
            execl("/bin/bash", "/bin/bash", "/home/cat/winch_ros2_ws/src/record_all.sh", bag_name.c_str(), (char *)0);
            _exit(EXIT_FAILURE);
            //  RCLCPP_INFO(this->get_logger(), "EXIT_FAILURE %d ", EXIT_FAILURE);
        }
        else if (process_pid_ < 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to fork process for rosbag recording.");
        }
        else
        {
            setpgid(process_pid_, process_pid_);
            is_recording_ = true;
            RCLCPP_INFO(this->get_logger(), "modify process_pid start recording");
        }
    }

    void stopRecording()
    {
        RCLCPP_INFO(this->get_logger(), "Stop record %d", process_pid_);
        if (process_pid_ > 0)
        {
            RCLCPP_INFO(this->get_logger(), "Stopping rosbag recording...");
            if (killpg(process_pid_, SIGINT) == -1)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to send SIGINT to process group %d", process_pid_);
                return;
            }

            RCLCPP_INFO(this->get_logger(), "SIGINT signal sent to process group %d", process_pid_);
            int status;
            pid_t result;
            int attempts = 0;
            const int max_attempts = 30;
            const int sleep_interval = 100000;

            do
            {
                result = waitpid(-process_pid_, &status, WNOHANG);
                if (result == 0)
                {
                    RCLCPP_INFO(this->get_logger(), "Waiting for process group %d to terminate...", process_pid_);
                    usleep(sleep_interval);
                    attempts++;
                }
            } while (result == 0 && attempts < max_attempts);

            if (result == -1)
            {
                RCLCPP_ERROR(this->get_logger(), "Error waiting for process group %d", process_pid_);
            }
            else if (result == 0)
            {
                RCLCPP_WARN(this->get_logger(), "Process group %d did not terminate, sending SIGKILL...", process_pid_);
                killpg(process_pid_, SIGKILL);
                waitpid(-process_pid_, &status, 0);
                RCLCPP_INFO(this->get_logger(), "Process group %d killed with SIGKILL", process_pid_);
            }
            else if (WIFSIGNALED(status) || WIFEXITED(status))
            {
                RCLCPP_INFO(this->get_logger(), "Process group %d terminated", process_pid_);
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "Process group %d did not terminate normally", process_pid_);
            }

            process_pid_ = -1;
            is_recording_ = false;
        }
    }

    ~RosbagRecorder()
    {
        if (is_recording_)
        {
            stopRecording();
        }
    }

private:
    // rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr trigger_sub_;
    rclcpp::Subscription<customized_msgs::msg::GcsCmd>::SharedPtr gcs_cmd_sub;
    bool is_recording_;
    pid_t process_pid_;
    std::string bag_dir = std::string(std::getenv("HOME")) + "/.ros";

    double get_free_space()
    {
        struct statvfs buffer;
        if (statvfs("/", &buffer) == 0)
        {
            unsigned long long free_space = buffer.f_bfree * buffer.f_frsize;
            return double(free_space) / 1024.0 / 1024.0 / 1024.0;
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to get filesystem stats.");
            return 0;
        }
    }

    // 自定义的文件排序函数，按文件名中的数字大小排序
    static bool compare_files(const std::string& a, const std::string& b) {
        auto extract_number = [](const std::string& str) -> int {
            size_t start = str.find_last_of('_') + 1; // 获取数字部分的位置
            return std::stoi(str.substr(start)); // 提取数字并转为整数
        };

        return extract_number(a) < extract_number(b); // 按数字大小排序
    }



    void delete_oldest_winch_log(double &free_space) {
        std::string log_prefix = "winch_log_";
        std::vector<std::string> log_files;

        // 获取目录下以 "winch_log_" 开头的文件
        for (const auto &entry : fs::directory_iterator(bag_dir)) {
            if (entry.is_regular_file()) {
                std::string filename = entry.path().filename().string();
                if (filename.rfind(log_prefix, 0) == 0) {
                    log_files.push_back(filename);
                }
            }
        }

        if (log_files.empty()) {
            RCLCPP_INFO(this->get_logger(), "No winch log files found.");
            return;
        }

        // 使用自定义的排序函数按数字大小排序文件
        std::sort(log_files.begin(), log_files.end(), compare_files);

        // 删除最旧的日志文件
        std::string oldest_log = log_files.front();
        struct stat file_info;
        if (stat(oldest_log.c_str(), &file_info) != 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to get file size for: %s", oldest_log.c_str());
        } else {
            double delete_file_size = double(file_info.st_size) / 1024.0 / 1024.0 / 1024.0;
            RCLCPP_INFO(this->get_logger(), "Deleting oldest winch log: %s of size %f GB", oldest_log.c_str(), delete_file_size);
            fs::remove(oldest_log);
            RCLCPP_INFO(this->get_logger(), "Delete %s complete", oldest_log.c_str());
            free_space += delete_file_size;
        }
    }

    
    int getMaxLogNumber()
    {
        int max_number = 0;
        std::string prefix ="winch_log_";
        for (const auto &entry : fs::directory_iterator(bag_dir))
        {
            // RCLCPP_INFO(this->get_logger(), "Checking file: %s", entry.path().string().c_str());
            // RCLCPP_INFO(this->get_logger(),"prefix %s",prefix.c_str());
            
            std::string filename = entry.path().filename().string();
            // RCLCPP_INFO(this->get_logger(),"filename %s",filename.c_str());
            // RCLCPP_INFO(this->get_logger(),"index %ld",filename.rfind(prefix, 0));
            if (filename.rfind(prefix, 0) == 0)
            {
                try
                {
                    int number = std::stoi(filename.substr(prefix.size()));
                    if (number > max_number)
                    {
                        max_number = number;
                    }
                }
                catch (const std::invalid_argument &e)
                {
                    continue;
                }
            }
            
        }
        // RCLCPP_INFO(this->get_logger(), "max_number %d", max_number);
        return max_number;
    }
};



int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RosbagRecorder>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}